from ros2webserver.baseserver.capability import Capability
import rclpy

class NavInitPose(Capability):

    def __init__(self, protocol):
        # Call superclass constructor
        Capability.__init__(self, protocol)
        protocol.register_operation("initializePose", self.init_pose)

    def init_pose(self, message):
        self.protocol.node_handle.get_logger().info(f"{message}")
        data = message['data']
        self.protocol.nav_handle.init_robot_pose(data['x'],data['y'],data['angle'])
        self.protocol.send({
            "op":"initializePose",
            "code":0,
            "msg":'ok',  
            "data":None
        })

    def finish(self):
        # self.protocol.node_handle.destroy_publisher(self._cmd_vel_pub)
        pass
